//OC1A = PB1 (taken)
//OC1B = PB2
//OC2A = PB3 (also MOSI, should not matter)
//OC2B = PD3 <-- 40KHz pulse
//OC0B = PD5 <-- burst timing


#define TIMER2_CLK 1
#define TIMER2_TOGGLETIME  250
#define TIMER2_PERIODCOUNT  5
//#define TIMER2_TOGGLECOUNT  18//2*period-2 (first and last toggle is manual)
#define TIMER2_TOGGLECOUNT  10

//for keeping track of periods we send
uint8_t g_timer2_count = 0;


//start the burst signal to the transducer
inline void startBurst(){
	//start the burst
    SET(PORTD, PD3);
	g_timer2_count = 0;
	TCNT2 = 1;
	TCCR2B |= TIMER2_CLK;
	OCR2A = TIMER2_TOGGLETIME;

}

void init_timers(uint8_t prescale0, uint16_t burst_time){
	//timer 0 handles the burst timing
	TCCR0A = (0<<COM0A1) | (0<<COM0A0) | (0<<COM0B1) | (0<<COM0B0) | (1<<WGM01) | (0<<WGM00);
	TCCR0B = (0<<WGM22) | prescale0;
	OCR0A = burst_time; 
	TIMSK0 |= (1<<OCIE0A);
		
	//timer 2 does the 40 KHz 
	//enable it's interrupt that triggers after ocr2a clockcycles
	TIMSK2 |= (1<<OCIE2A);
	//toggle OC2B on match, set CTC mode
	TCCR2A |= (0<<COM2A1) | (0<<COM2A1) | (0<<COM2B1) | (0<<COM2B0) | (1<<WGM21) | (0<<WGM20);
	//set prescaler to TIMER2_CLK
	TCCR2B |= (0<<WGM22) | TIMER2_CLK;
	//250 * 50ns = 12.5us (if we toggle that comes down to 25us periods)
	OCR2A = TIMER2_TOGGLETIME;
	g_timer2_count = 0;
	//enable OC2B pin as output
	SET(DDRD, PD3);
}

ISR(TIMER2_COMPA_vect){
	//count a toggle
	g_timer2_count++;
	//TOG(PORTD, PD3);
	//if we're not still below the required number of toggles
    if(g_timer2_count >= TIMER2_TOGGLECOUNT){
		TCCR2B = 0;
        CLR(PORTD, PD3);
	}

    

}

ISR(TIMER0_COMPA_vect){
	startBurst();
}
